Mobile entity position estimation device and position estimation method

ABSTRACT

Improvement in the accuracy of estimating the position of a mobile entity even while traveling or if there is an error in the calibration performed utilizing: a mobile entity; an imaging device provided in the mobile entity; and an information processing device for determining a first movement amount by which a detection point that is the same object has moved on the basis of a first image and a second image acquired by the imaging device and a second movement amount by which the mobile entity has moved while the first image and the second image were acquired, determining the accuracy of recognizing the detection point acquired by the imaging device on the basis of the first movement amount and the second movement amount, and estimating the position of the mobile entity on the basis of the accuracy of recognition and position information that pertains to the detection point.

TECHNICAL FIELD

The present invention relates to a position estimation device and aposition estimation method of a mobile entity such as a robot and anautomobile.

BACKGROUND ART

Autonomous travel technology and drive support technology for a mobileentity such as a robot and an automobile to collect information aboutthe surroundings, to estimate the current position and the running stateof the mobile entity, and to control the running of the mobile entityhave been developed.

Various types of sensors are used as means for collecting information onthe surroundings and position of a mobile entity. Sensors for measuringsurrounding information include a laser sensor and a millimeter-waveradar, in addition to an imaging device such as a camera. A globalpositioning system (GPS) or an inertial measurement unit (IMU) is usedas a sensor for measuring the position of the mobile entity.

In the autonomous travel control, the control device mounted on themobile entity integrates the velocity or angular velocity of the mobileentity calculated, for example, by the IMU or uses GPS positioning toestimate the position of the mobile entity itself (self position). Inaddition, if there is no map information or landmarks and GPS cannot beused either, the simultaneous localization and mapping (SLAM) method forcreating a map of the environment during travel while estimating therelative position with the objects existing around the mobile entity isused. However, the error of the relative position estimated by the SLAMmethod is accumulated in a time series, as a result, the positioncorrection is essential. In this position correction, for example,collecting surrounding information using a laser sensor, a camera, orthe like, detecting landmarks such as road paint or signs serving as areference for position estimation, and with the control device,comparing the position of the detected landmark with the map informationcorrects the current position of the mobile entity. Therefore, when thedetected landmark has a position error, the position of the mobileentity cannot be corrected accurately in some cases.

In particular, when recognizing landmarks with a monocular camera, inorder to geometrically calculate the distance to the recognizedlandmark, it is necessary to accurately transform the position of thelandmark on the image of the camera into the position of the actuallandmark. Here, in order to estimate the position of the landmark withhigh accuracy, it is necessary to execute internal parameter calibrationand external parameter calibration of the camera. The internal parametercalibration corrects the lens distortion of the camera and calculatesthe focal length. On the other hand, the external parameter calibrationdetermines the current installation height and angle of the camera.

For example, a camera mounted on a vehicle is attached to the vehicle ata position and an angle in accordance with predetermined design valuesof the camera, and at this time, an error may occur, and the recognitionaccuracy of the surroundings by the camera is reduced. In order tocorrect this error, generally, a calibration index printed on paper or aboard is precisely set at a determined position and photographed, and inorder that the photographed image matches the image photographed from apredetermined position, parameters of the camera are corrected. It iscommon to calibrate vehicles before shipping them at factories and thelike, but since the attitude of the vehicle changes due to differencesin the number of passengers, differences in sitting places, differencesin how to load luggage, and the like, it is necessary to performcalibration even after shipment from the factory in some cases.

Here, for example, PTL 1 discloses an invention that relates to acalibration device for performing calibration of a camera mounted on avehicle, and includes an image acquisition unit for acquiring an imageoutside the vehicle and a calibration unit for calibrating at least onecamera parameter of a roll angle αnd a pitch angle of the camera using acorresponding feature point between the image before the attitude changeand the image after the attitude change of the vehicle.

CITATION LIST Patent Literature

PTL 1: JP 2017-78923 A

SUMMARY OF INVENTION Technical Problem

In PTL 1, calibration of the monocular camera is performed using imagesbefore and after the attitude change, but since the effects of vibrationduring travel and the effects of errors during performing calibrationare not considered and are not reflected in the camera's internalparameters or the camera's external parameters, the problem remains thatthe recognition accuracy of the position of the landmark recognized bythe camera decreases. When the recognition accuracy of the position ofthe landmark decreases, the estimation accuracy of the position of themobile entity itself also decreases.

Thus, an object of the present invention is to improve the accuracy ofestimating the position of a mobile entity even when travel is inprogress or there is an error in the calibration performed.

Solution to Problem

In order to solve the above problem, the present invention includes: amobile entity; an imaging device provided in the mobile entity; and aninformation processing device for determining a first movement amount bywhich a detection point that is the same object has moved on the basisof a first image and a second image acquired by the imaging device and asecond movement amount by which the mobile entity has moved during theacquisition of the first image and the second image, determining theaccuracy of recognizing the detection point acquired by the imagingdevice on the basis of the first movement amount and the second movementamount, and estimating the position of the mobile entity on the basis ofthe accuracy of recognition and position information that pertains tothe detection point.

Advantageous Effects of Invention

According to the present invention, the accuracy of estimating theposition of a mobile entity can be improved even when travel is inprogress or there is an error in the calibration performed.

BRIEF DESCRIPTION OF DRAWINGS

FIG. 1 is a configuration diagram of a position estimation device of amobile entity according to an embodiment.

FIG. 2 is a flowchart showing an image processing procedure.

FIG. 3 is a diagram illustrating a moving point on an image.

FIG. 4 is a diagram illustrating distance estimation of a moving pointby an imaging device.

FIG. 5 is a diagram illustrating distance estimation of a moving pointby an imaging device.

FIG. 6 is a diagram illustrating distance estimation of a moving pointby an imaging device.

FIG. 7 is a diagram illustrating a principle related to positionestimation.

FIG. 8 is a diagram illustrating a principle related to positionestimation.

FIG. 9 is a diagram illustrating a principle related to positionestimation.

FIG. 10 is a diagram illustrating an application example.

FIG. 11 is an explanatory diagram of calibration in an embodiment.

FIG. 12 is a flowchart of calibration.

FIG. 13 is a diagram illustrating details of an image transformationstep.

DESCRIPTION OF EMBODIMENTS

Hereinafter, embodiments of the present invention will be described withreference to the drawings. It should be noted that the following ismerely an example, and is not intended to limit the content of theinvention to the following specific aspects. The invention itself can beimplemented in various modes as long as it conforms to the contentsdescribed in the claims.

First Embodiment

FIG. 1 is a configuration diagram of a position estimation device 1 of amobile entity according to an embodiment. The position estimation device1 is mounted on a mobile entity 100 such as an automobile or a robot.The position estimation device 1 includes one or more imaging devices 12a, 12 b, . . . , 12 n, and an information processing device 13. Theimaging devices 12 a, 12 b, . . . , 12 n are still cameras or videocameras, for example. In addition, the imaging devices 12 a, 12 b, . . ., 12 n may be monocular cameras or compound eye cameras.

The information processing device 13 processes the images captured bythe imaging devices 12 a, 12 b, . . . , 12 n to calculate the positionor the movement amount of the mobile entity 100. The informationprocessing device 13 may perform display in accordance with thecalculated position or movement amount, or may output a signal relatedto control of the mobile entity 100.

The information processing device 13 is, for example, a generalcomputer, and includes an image processing unit 14 for processing animage captured by the imaging devices 12 a, 12 b, . . . , 12 n, acontrol unit 15 (CPU) for performing control based on the result of theimage processing unit, a memory 16, a display unit 17 such as a display,and a bus 18 for interconnecting these components. The image processingunit 14 and the control unit 15 execute a predetermined computerprogram, whereby the information processing device 13 performs thefollowing processing.

The imaging device 12 a is installed in the front of the mobile entity100, for example. The lens of the imaging device 12 a is directedforward of the mobile entity 100. The imaging device 12 a captures adistant view ahead of the moving entity 100, for example. The otherimaging devices 12 b, . . . , 12 n are installed at positions differentfrom that of the imaging device 12 a, and capture imaging directions orregions different from that of the imaging device 12 a. The imagingdevice 12 b may be installed in the rear of the mobile entity 100 to bedirected downward, for example. The imaging device 12 b may capture anear view behind the moving entity 100.

When the imaging device 12 a is a monocular camera, if the road surfaceis flat, the positional relationship (x, y) between the pixel positionon the image and the actual ground position is constant, so that thedistance from the imaging device 12 a to the feature point can begeometrically calculated. When the imaging device 12 a is a stereocamera, the distance to a feature point on the image can be measuredmore accurately. In the following description, an example in which acamera having a monocular standard lens is adopted will be described,but another camera (such as a camera having a wide-angle lens or astereo camera) may be used. In addition, the objects to be captured bythe imaging devices 12 a, 12 b, . . . , 12 n at a certain time may bedifferent from each other. For example, the imaging device 12 a maycapture a distant view ahead of the mobile entity 100. In this case, afeature point such as a three-dimensional object or a landmark forposition estimation may be extracted from the image obtained bycapturing the distant view. The imaging device 12 b may capture a nearview such as a road surface around the mobile entity 100. In this case,a white line around the mobile entity 100, road surface paint, or thelike may be detected from the image obtained by capturing the near view.

In addition, the imaging devices 12 a, 12 b, . . . , 12 n may beinstalled on the mobile entity 100 under the conditions of not beingsimultaneously affected by environmental disturbances such as rain andsunlight. For example, the imaging device 12 a may be installed to bedirected forward in the front of the mobile entity 100, while theimaging device 12 b may be installed to be directed backward or downwardin the rear of the mobile entity 100. Thus, for example, even whenraindrops adhere to the lens of the imaging device 12 a during rainfall,the raindrops do not easily adhere to the lens of the imaging device 12b directed in a direction opposite to the traveling direction ordownward. Therefore, even if the image captured by the imaging device 12a is unclear due to the effect of raindrops, the image captured by theimaging device 12 b is less likely to be affected by raindrops.Alternatively, even if the image of the imaging device 12 a is uncleardue to the effect of sunlight, the image captured by the imaging device12 b may be clear.

In addition, the imaging devices 12 a, 12 b, . . . , 12 n may captureimages under different capturing conditions (aperture value, whitebalance, and the like). For example, mounting an imaging device whoseparameters are adjusted for a bright place and an imaging device whoseparameters are adjusted for a dark place to make imaging possibleregardless of the brightness of the environment may be used.

The imaging devices 12 a, 12 b, . . . , 12 n may capture images whenreceiving a shooting start command from the control unit 15 or at afixed time interval. The data on and the imaging time of the capturedimage are stored in the memory 16. It should be noted that the memory 16includes a main storage device (main memory) of the informationprocessing device 13 and an auxiliary storage device such as a storage.

The image processing unit 14 performs various pieces of image processingbased on the image data and the imaging time stored in the memory 16. Inthis image processing, an intermediate image is created and stored inthe memory 16, for example. The intermediate image may be used fordetermination and processing by the control unit 15 and the like besidesthe processing by the image processing unit 14.

The bus 18 can include an Inter Equipment Bus (ZEBUS), a LocalInterconnect Network (LIN), a Controller Area Network (CAN), or thelike.

The image processing unit 14 identifies a plurality of positioncandidates of the mobile entity based on the image captured by theimaging device 12, and estimates the position of the mobile entity 100based on the plurality of position candidates and the moving speed ofthe mobile entity 100.

In addition, the image processing unit 14 may process an image capturedby the imaging device 12 while the mobile entity 100 travels to estimatethe position of the mobile entity 100, or may calculate the movementamount of the mobile entity 100 based on the video image captured by theimaging device 12 to estimate the current position by adding themovement amount to the start point, for example.

The image processing unit 14 may extract a feature point from each frameimage of the video image. The image processing unit 14 further extractsthe same feature point in the subsequent frame images. Then, the imageprocessing unit 14 may calculate the movement amount of the mobileentity 100 based on tracking the feature points.

The control unit 15 may output a command related to the moving speed tothe mobile entity 100 based on the result of the image processing by theimage processing unit 14. For example, according to the number of pixelsof a three-dimensional object in the image, the number of outliers amongthe feature points in the image, the type of image processing, or thelike, the control unit 15 may output commands to increase, to decrease,or to maintain the moving speed of the mobile entity 100.

FIG. 2 is a flowchart illustrating an image processing procedureperformed by the image processing unit 14.

The image processing unit 14 acquires image data captured by the imagingdevices 12 a, 12 b, . . . , 12 n from the memory 16 (S21). The imagedata acquired in step S21 may be image data including only one image ora plurality of images of the images captured by the imaging devices 12a, 12 b, . . . , 12 n. In addition, in step S21, not only the latestimage captured by each of the imaging devices 12 a, 12 b, . . . , 12 n,but also an image captured in the past may be used.

Next, the image processing unit 14 extracts a moving point in each ofthe acquired frame images (S22). The moving point may be a feature pointsuch as an edge in the image, a corner, or a maximum value or minimumvalue of the pixel intensity, for example. For the extraction of thefeature point, a technique such as Canny, Sobel, FAST, Hessian, andGaussian only has to be used. A specific algorithm is appropriatelyselected according to the characteristics of the image. In addition, themoving point may be a representative point of the recognized landmark(center, corner, or the like of the landmark). Conventional imagerecognition techniques such as deep learning and template matching onlyhave to be used for landmark recognition and representative pointextraction. Details of the moving point will be described below.

The image processing unit 14 tracks the moving points extracted in eachframe image according to the time series of the frame images (S23). Fortracking, techniques have only to be used such as Lucas-Kanade method,Shi-Tomasi method, and Direct Matching method. In addition, the trackingin step S23 is not limited to the moving points of the frames acquiredimmediately before or immediately after, and may be the moving points ofthe frames acquired at intervals of several frames. The specificalgorithm is appropriately selected according to the moving point of theimage.

Next, a calculation for converting the respective moving points trackedin step S23 into the movement amount in the real world is performed(S24). The difference between the pixel position on the image obtainedby the tracking in step S23 and the pixel position of the previous framebefore that is calculated, and the difference is transformed into unitsof meters.

In step S25, the movement amount of the mobile entity on which theimaging device is mounted is estimated. In this step, the actualmovement amount of the mobile entity 100 between the images capturedthis time and last time by the imaging devices 12 a, 12 b, . . . , 12 nis estimated. For estimating the actual movement amount of the mobileentity 100, techniques such as GPS information, odometry, imageodometry, and SLAM method have only to be adapted. In addition, a timeseries filter for estimating the movement amount this time based on themovement amount in the past may be used. In addition, also the movementamount of the mobile entity 100 may be estimated by combining theabove-described sensor information and the filter. In the end, anysensor or combination may be used as long as a method can estimate themovement amount of the mobile entity 100 between the previous frame andthe frame this time. The timing of performing step S25 may beimmediately after step S24, or may be performed in parallel from stepS21 to step S24. Step S25 may be performed at any time before theprocessing of step S26 starts.

In step S26, the accuracy of the moving point tracked in step S23 isestimated using the movement amount information about the moving pointobtained in step S24 and the movement amount information about themobile entity 100 obtained in step S25. Details of step S26 will bedescribed below.

Based on the accuracy estimated in step S26, the calibration of theimaging devices 12 a, 12 b, . . . , 12 n is performed if necessary(S27). Execution in step S27 is optional, and details will be describedbelow.

FIG. 3 is a diagram illustrating a moving point on an image. The image210 is an image acquired in step S21 by the imaging devices 12 a, 12 b,. . . , 12 n. The coordinates 211 represented by (u, v) are a coordinatesystem in the image 210. The road 212 is a road appearing in the image210 while the mobile entity 100 is in travel.

It is assumed that the mobile entity 100 travels on the road 212 and alandmark 213 on the road surface of the road 212 appears in the image210 at a certain time. At this time, when the moving point is extractedin step S22, it is assumed that feature points 214 are extracted fromthe landmark 213. It should be noted that the feature points are notlimited to the edges and corners in the image, and may be the maximumvalues or the minimum values of the pixel intensity. In addition, whenlandmarks are matched to the map, for simplicity, the recognizedlandmark may be represented by a representative point (center, corner,and the like of the landmark), and a representative point 216 isrepresented as a representative of the landmark 214. Here, the featurepoint 214, the feature point 215, and the representative point 216extracted in step S22 on the image 210 are stationary points in the realworld, and since the feature point 214, the feature point 215, and therepresentative point 216 move with respect to the imaging devices 12 a,12 b, . . . , 12 n fixed to the mobile entity 100, the feature point214, the feature point 215, and the representative point 216 are alldefined as “moving points”.

The distance estimation of the moving point detected by the imagingdevices will be described with reference to FIGS. 4 to 6. FIGS. 4 to 6are diagrams illustrating the distance estimation of the moving point bythe imaging devices.

The installation angle αN 30 a in FIG. 4 is the installation angle ofthe imaging devices 12 a, 12 b, . . . , 12 n with respect to the roadsurface on which the mobile entity 100 is in travel. The height H_(N) 31a is the installation height of the imaging devices 12 a, 12 b, . . . ,12 n with respect to the road surface on which the mobile entity 100 isin travel. For simplicity, the imaging devices 12 a, 12 b, . . . , 12 nwill be described with one imaging device 12 as a representative. Thecoordinates 32 a are coordinates in units of meters fixed to the mobileentity 100, and the point 33 a is one point on the road surface. At thistime, the point 33 a on the image 34 a is extracted as a moving point instep S22, and is represented as a pixel 35 a. In this case, it isassumed that the imaging device has been calibrated, and therelationship between the actual position (meter) of the point 33 a andthe moving point 35 a (pixel) on the image is obtained by using theinstallation angle α_(N) 30 a and the installation height H_(N) 31 a.Therefore, when the installation angle αN 30 a and the installationheight H_(N) 31 a of the imaging device 12 are constant, therelationship between the coordinates 32 a (meters) and the image (pixel)is constant, and transformation from meters into a pixel can be easilyperformed. In addition, if the installation angle α_(N) 30 a and theinstallation height H_(N) 31 a of the imaging device 12 are constant,even if the image 34 a is transformed into a bird's-eye view image orthe like, if the transformation parameters are known, the relationshipbetween the coordinates 32 a (meters) and the image (pixel) does notchange.

On the other hand, FIG. 5 illustrates a case different from the casewhere the calibration of the imaging device is performed in the state inFIG. 4. It is assumed that the installation angle α_(N) 30 a of theimaging device 12 becomes the installation angle α′N 30 b and the heightH_(N) 31 a becomes the height H_(N) 31 b due to the difference in thevibration and the number of passengers. In this state, the point 33 a isextracted as the moving point 35 b on the image 34 b, but sincecalibration has been performed based on coordinates 32 a, the distanceto the virtual point 33 b with respect to the coordinates 32 a iscalculated. However, in this case, since the distance to the point 33 awith respect to the coordinates 32 b is accurate, an error occurs whenthe distance with respect to the coordinates 32 a is calculated. Forsimplicity, the case where the pitch angle of the mobile entity 100 istransformed has been described with reference to FIG. 5, but even whenthe roll angle or the yaw angle of the mobile entity 100 changes, theabove-described principle does not change.

FIG. 6 shows the same installation angle α_(N) 30 a, height H_(N) 31 a,coordinates 32 a, and point 33 a as in FIG. 4. The image 34 c acquiredby the imaging device 12 in step S21 represents an image whosedistortion cannot be corrected even after calibration. In the image 34 cwhose distortion has not been corrected, the point 33 a appears as apixel 35 c, and appears at a position different from that of the pixel35 a of the image 34 a whose distortion has been corrected. Therefore,even if the moving point 33 a is extracted from the image 34 c whosedistortion has not been corrected, when the pixel 35 c is transformedinto meters and the actual position is estimated, the error is large.

Next, the principle of the present embodiment will be described withreference to FIGS. 7 to 9. FIGS. 7 to 9 are diagrams illustrating theprinciple related to position estimation.

FIG. 7 shows a state in which the mobile entity 100 travels at the sameinstallation angle α_(N) 30 a and height H_(N) 31 a as when calibrationis performed. It is assumed that the distortion of the imaging devices12 a, 12 b, . . . , 12 n has been corrected. The points 40 are featurepoints on the road surface on which the mobile entity 100 is in travel.For simplicity, the imaging devices 12 a, 12 b, . . . , 12 n will bedescribed with one imaging device 12 as a representative. The image 41 ais a bird's-eye view image captured by the imaging device 12, and thecoordinates are (u′, v′). To simplify the description of the principle,the image 41 a is a bird's-eye view image, but any image may be used aslong as the relationship between a pixel and meters is known. In stepS22, each point 40 is extracted on the image 41 a as a moving point 42a. After the mobile entity 100 has traveled the movement amount D_(N)43, when each extracted moving point 42 a is tracked in step S23 and themovement amount is calculated in step S24, all of each movement amount44 a on the bird's-eye view image 41 a are constant. In addition, wheneach movement amount 44 a on the bird's-eye view image 41 a istransformed from a pixel into meters, each movement amount becomes amovement amount the same as the movement amount D_(N) 43 of the mobileentity 100.

On the other hand, FIG. 8 shows a state in which the mobile entity 100is in travel at an installation angle α′_(N) 30 b and a height H′_(N) 31b different from those at the time of calibration. It is assumed thatthe distortion of the imaging device 12 has been corrected. After themobile entity 100 has traveled the movement amount D_(N) 43, when eachextracted moving point 42 b is tracked in step S23 and the movementamount is calculated in step S24, the movement amount 44 b of eachmoving point 42 b on the bird's-eye view image 41 b differs depending onthe position of the image, and the movement amount of a point close tothe imaging device appears to be larger than the movement amount of apoint far from the imaging device. When the moving point 40 isphotographed at the installation angle α′_(N) 30 b and the height H′_(N)31 b in this state, since the movement amount 44 b on the image 41 b iscalculated to be mistaken for the plane 45, the movement amount of themoving point 42 b close to the mobile entity 100 appears to be large,and the movement amount 44 b of the moving point 42 b far from themobile entity 100 appears to be small. Therefore, when each movementamount 44 b on the bird's-eye view image 41 b is transformed from apixel into meters, each movement amount 44 b becomes a movement amountdifferent from the movement amount D_(N) 43 of the mobile entity 100.Therefore, when the movement amount of the moving point extracted instep S21 and step S22 is not constant, calibration is not performed, andwhen the distance to a point on the road surface is calculated in thisstate, the distance error is large.

FIG. 9 represents the installation angle α_(N) 30 a, height H_(N) 31 a,point 40, moving point 42 a, and movement amount D_(N) 43 of the mobileentity 100 the same as those in FIG. 7. It is assumed that thedistortion of the bird's-eye view image 41 c has not been corrected.When the mobile entity 100 travels the movement amount D_(N) 43 in thisstate, the movement amount of each moving point 42 a on the bird's-eyeview image 41 c is the movement amount 44 c. Due to the distortion, themovement amount 44 c differs depending on the area of the bird's-eyeview image 41 c of the moving point 42 a, and when the distance to themoving point in that area is calculated, the distance error is large.

An application example in the present embodiment will be described withreference to FIG. 10. FIG. 10 is a diagram illustrating an applicationexample. The traveling environment 50 is an environment in which themobile entity 100 travels. For simplicity, it is assumed that thetraveling environment 50 is a parking lot. The map information 51 is amap of the traveling environment 50. The traveling environment 50includes stationary landmarks such as lanes, parking frames, and signsof the traveling environment, and has accurate position information oneach. The coordinate system of the map information 51 may represent theabsolute position of the world or the relative position of a certainarea. In the end, any coordinates may be used as long as the currentposition of the mobile entity 100 can be accurately displayed.

The imaging range 52 is an imaging range of the imaging devices 12 a, 12b, . . . , 12 n. For simplicity, it is assumed that there are twoimaging devices 12 a, 12 b, . . . , 12 n, which are installed on theright and left sides of the mobile entity 100 and face the right andleft sides of the mobile entity 100. In this application example, theself position estimation of the mobile entity 100 is performed using theabove-described accuracy estimation while the position of the landmarkrecognized by the two imaging devices is compared with the mapinformation 51.

The moving point 53 is a point when the mobile entity 100 enters thetraveling environment 50. For simplicity, it is assumed that the movingpoints in this application example are all corners of the parking frame.It is assumed that images acquired by the imaging devices after themobile entity 100 has traveled the movement amount 54 are a bird's-eyeview image 55 a and a bird's-eye view image 55 b. It is assumed that themovement amount of the moving point 53 on the images 55 a and 55 b afterthe mobile entity 100 has traveled the movement amount 54 is a movementamount 56 a. Here, the movement amount 56 a is a movement amount of themoving point extracted in step S22 using the imaging devices and themoving point when the extracted moving point is tracked in step S23.

Since the movement amount 56 a has become the same movement amount asthe actual movement amount 54 of the mobile entity 100 obtained in stepS25, it is determined that the area around the movement amount 56 a onthe image is highly accurate (there is little recognition error).Therefore, when the self position of the mobile entity 100 is estimatedfrom the map information 51 based on the calculated position of themoving point 53, the estimation can be performed with high accuracy.

The moving points 57 b and 57 c are moving points after the mobileentity 100 has left the start position. It is assumed that the imageacquired by the imaging device after the mobile entity 100 has traveledthe movement amount d_(N) 58 is a bird's-eye view image 55 c. It isassumed that the movement amount of the moving point 57 b on the image55 c after the mobile entity 100 has traveled the moving amount d_(N) 58is a moving amount 56 b and the movement amount of the moving point 57 cis a movement amount 56 c. Here, the movement amounts 56 b and 56 c aremovement amounts of the moving points when the moving points areextracted from the moving points 57 b and 57 c in step S22 using theimaging device, and the extracted moving points are tracked in step S23.It is assumed that when the movement amount 56 b is transformed intometers based on the installation height and angle of the imaging device,the movement amount has become a movement amount the same as themovement amount d_(N) 58 of the mobile entity 100. Therefore, when themobile entity 100 travels the movement amount d_(N) 58, since the areaaround the movement amount 56 b on the image 55 c is highly accurate, itis determined that the accuracy of the detected moving point 57 b ishigh, the position of the detected moving point 57 b is matched with themap information 51, and position estimation is performed. On the otherhand, since the movement amount 56 c is different from the movementamount d_(N) 58, it is determined that the accuracy of the area on theimage around the movement amount 56 c is low, therefore, matching is notperformed with the map information 51, and tracking is performed in stepS23 over several frames of the imaging device until accuracy isimproved. As a method for increasing the accuracy of the low accuracyarea, a time series filter such as a Kalman Filter is applied, forexample. Details will be described below.

Therefore, the self position of the mobile entity 100 can be estimatedwith high accuracy from the map information 51 based on the accuracydetermined as the calculated positions of the moving points 57 b and 57c.

The accuracy determined as described above is used for positionestimation as a weight w_(N, p), for example. As shown in Formula (1),the difference between the actual movement amount d_(N) 58 of the mobileentity 100 and the movement amount I_(N, p) (p=1, 2, . . . , and thenumber of moving points) of the moving point is assumed to beerror_(N, p).1/w _(N,p)=error_(N,p) =|d _(N) −I _(N,p)|  Formula (1)

In addition, the error in Formula (1) may be calculated not by using themeters but by using the ratio of the movement amount d_(N) 58 of themobile entity 100 as shown in Formula (2).1/w _(N,p)=error_(N,p) =|d _(N) −I _(N,p) |/d _(N)×100  Formula (2)

The weights in Formulae (1) and (2) may be substituted into parametersof a time-series filter such as a Kalman Filter, for example. When theKalman Filter is used, it is necessary to set the error of the sensor orthe system as the deviation ON, and the above-described errorerror_(N, p) is substituted into the deviation σ_(N) as shown in Formula(3).

In the case of FIG. 10, the above-described deviation σ_(N) is indicatedby deviation 59 b and deviation 59 c. Since the weight of the movementamount 56 b is high, the deviation 59 b is small, and since the weightof the movement amount 56 c is low, the deviation 59 c is large.σ_(N)=error_(N,p)  Formula (3)

In addition, since pixels are geometrically transformed into metersbased on the installation height and angle of the imaging device, if thelandmark is far from the imaging device, the landmark is likely to beaffected by the vibration of the mobile entity 100 and the like, so thatthe error is likely to increase. Therefore, the deviation σ_(N) withrespect to the distance/pixel position may be set without calculatingthe above-described error error_(N, p). For example, assuming that thewidth (u′ direction) of the bird's-eye view images 55 a and 55 bcaptured by the imaging device is denoted by W, and the height (v′direction) is denoted by V, since (u′, v′)=(W/2, V) is the pixelposition closest to the mobile entity 100, the area has the smallesterror. On the other hand, since (u′, v′)=(0, 0) or (u′, v′)=(W, 0) isthe pixel position farthest from the mobile entity 100, the area has thelargest error. Therefore, assuming that the maximum deviation in the u′direction is σ_(u, max), the deviation σ with respect to the pixel u canbe obtained from Formula (4).σ_(u)=σ_(u,max) |W/2−u′|/(W/2)  Formula (4)

Assuming that the maximum deviation in the v′ direction is σ_(v, max) inthe same manner as in the u direction, the deviation σ_(v) with respectto the pixel v′ can be obtained from Formula (5).σ_(v)=σ_(v,max) |V−v′|/(V)  Formula (5)

By combining σ_(u) and σ_(v), the deviation σ_(N) can be obtained, forexample, from Formula (6).σ_(N)=σ_(u)+σ_(v)  Formula (6)

In addition, the deviation σ_(N) may be calculated from Formula (7) bycombining σ_(u) and σ_(v).σ_(N)=σ_(u)σ_(v)  Formula (7)

In addition, weights m (1, 2, . . . , m) may be assigned to σ_(u) andσ_(v), and deviation σ_(N) may be calculated from Formula (8).σ_(N)=(σ_(u) ^(m)+σ_(v) ^(m))^(1/m)  Formula (8)

In addition, when the above-described error_(N, p) is calculated, thedeviation σ_(N) may be calculated as shown in Formula (9) by combiningerror_(N, p) with σ_(u) and σ_(v).σ_(N)=error_(N,p)(σ_(u) ^(m)+σ_(v) ^(m))^(1/m)  Formula (9)

The calculation of the deviation σ_(N) may be any of the Formulae (1) to(9) as long as the combination includes d_(N), I_(N, p), error_(N, p),σ_(u), σ_(v), u, and v. In addition, the above-described setting ofσ_(u, max) and σ_(v, max) may be set to fixed values or set empirically.In addition, since σ_(u, max) and σ_(v, max) are not necessarilyσ_(u, max)=σ_(v, max), different parameters may be set.

When only one moving point is present in the imaging device (p=1), theself position (X, Y)_(N) and the azimuth (θ)_(N) of the mobile entity100 are calculated based on the position (X, Y, θ)_(p=1) of the movingpoint as expressed by Formula (10).(X,Y,θ)_(N) =w _(N,p=1)(X,Y,θ)_(p=1)  Formula (10)

In addition, when there are a plurality of moving points in the imagingdevice, the self position (X, Y, θ)_(N) of the mobile entity 100 can beobtained from Formula (11).(X,Y,θ)_(N)=[W _(N,1(X,Y,θ)1) + . . . +w _(N,p(X,Y,θ)p)]/(w _(N,1) + . .. +w _(N,p))  Formula (11)

When the self position of the mobile entity 100 is estimated, anycombination of the calculated positions (X, Y, θ)1, . . . , (X, Y, θ)pand weights w_(N, 1), . . . , w_(N,p) may be used other than the aboveFormulae (10) and (11).

In addition, in the present embodiment, it is assumed that the movingpoint is defined as the corner of the parking frame, and the movingpoint can be recognized by the imaging device 12 using the imageprocessing technique without any problem. On the other hand, in actualparking lots and roads, there are moving objects such as pedestrians andother vehicles, as a result, it may be difficult to recognize thecorners of the parking frame. However, since such an obstacle is higherthan the road surface, even if the movement amount of the moving pointis calculated in steps S21 to S24, the movement amount I_(N,p)increases, and the error error_(N, p) also increases. Therefore, even ifan obstacle is erroneously recognized as a moving point, since theweight w_(N, p) becomes low, the result does not affect the positionestimation result.

The calibration according to the present embodiment will be describedwith reference to FIGS. 11 and 12. FIG. 11 is an explanatory diagram ofthe calibration in the present embodiment, and FIG. 12 is a flowchart ofthe calibration.

Images 60 a, 60 b, . . . , 60N in FIG. 11 are images captured in stepS21 in time series at time t0, t1, . . . , tN by the imaging devices 12a, 12 b, . . . , 12 n. The moving points 61 a, 61 b, . . . , 61N aremoving points extracted from the images 60 a, 60 b, . . . , 60N in stepS22. The movement amounts 62 a, 62 b, . . . , 62N are the calculatedmovement amounts of the moving points 61 a, 61 b, . . . , 61N in stepS23 and step S24. Here, it is assumed that calibration has not beenperformed, and the movement amounts 62 a, 62 b, . . . , 62N are notalways constant.

The image 63 is an image after the calibration S27 is performed. Themovement amount 64 is a movement amount of the moving point on the image63 calculated in steps S21 to S24 after the calibration step S27 isperformed. Since the image 63 is an image after the calibration has beenperformed, the movement amount 64 of the moving point calculated insteps S21 to S24 is constant.

Steps S65 to S68 in FIG. 12 are processing in the calibration S27. StepS65 is a step of storing the information calculated in steps S21 to S25.Images 60 a, 60 b, . . . , 60N, moving points 61 a, 61 b, . . . , 61N,movement amounts 62 a, 62 b, . . . , 62N, the movement amount of themobile entity 100, and the like are stored in the memory 16.

Step S66 is a step of performing image transformation on the images 60a, 60 b, . . . , 60N and the moving points 61 a, 61 b, . . . , 61Ntracked in step S23. The image transformation of S66 is, for example, anaffine transformation or a perspective transformation, and rotation andtranslation of the images 60 a, 60 b, . . . , 60N and the moving points61 a, 61 b, . . . , 61N tracked in step S23 are transformed. Details ofstep S66 will be described below.

In step S67, from the images 60 a, 60 b, . . . , 60N and the movingpoints 61 a, 61 b, . . . , 61N tracked in step S23 transformed in stepS66, the respective new moving amounts I_(N, p,i) (i=1, . . . , thenumber of calibration) of the moving points 61 a, 61 b, . . . , 61N arecalculated. As shown in the Formula (12), the errors E_(N, p, i) betweenthe newly calculated I_(N, p, i) and the movement amounts d₀, . . . ,d_(N) of the mobile entity 100 stored in time series in step S65 arecalculated.E _(N,p,i) =|d _(N) −I _(N,p,i)|  Formula (12)

Step S68 is a step of comparing the errors E_(N, p, i) calculated instep S67 with a preset threshold value min_(error). If the errorE_(N, p, i) calculated in step S67 is smaller than min_(error), step S27ends, and if the error E_(N, p, i) calculated in step S67 are largerthan min_(error), the process returns to step S66.

At least two frames are essential for the number of frames N, and themaximum value of N may be set based on the number of moving pointsobtained in time series in steps S21 to S24, for example. Basically, ifthe number of moving points is large, the calibration error is small,but the processing load is large. Therefore, for example, if the numberof moving points obtained in time series in steps S21 to S24 is largerthan a preset threshold value, calibration is performed using all thenumber of frames and moving points obtained in time series in steps S21to S24 up to that time. In addition, since there is a high possibilitythat the parameter becomes different from the calibration parameterperformed last time depending on the traveling speed of the mobileentity 100, N may be set according to the speed of the mobile entity100. For example, if the speed of the mobile entity 100 is low, sincethe calibration parameter does not change significantly, N is set high,and if the speed of the mobile entity 100 is high, since the calibrationparameter changes significantly, N is set low, calibration is performedat high frequency. In addition, N may be set based on the travelingtime. For example, if the processing load has a margin, the calibrationis performed every several ms or tens of ms, and if the processing loaddoes not have a margin, the calibration is performed every severalhundred ms or several seconds.

Details of step S66 will be described with reference to FIG. 13. FIG. 13is a diagram illustrating details of the image transformation step.

The bird's-eye view image 70 a is an image captured by the imagingdevice and transformed into a bird's-eye view image. It is assumed thatthe moving point extracted in step S22 is a moving point 71 and themovement amount of each of the moving points 71 obtained in steps S21 toS24 is a movement amount 72 a. When the movement amount 72 a of themoving point 71 is obtained in steps S21 to S24, it is assumed that thecalibration of the imaging device has been performed, and the movementamount 72 a of each of the moving points 71 obtained in steps S21 to S24is constant. Since the movement amounts 72 a of the moving points 71calculated in steps S21 to S24 are all constant, the roll, pitch, andyaw of the imaging device are the same as when the calibration isperformed. On the other hand, even if the movement amounts 72 a of themoving points 71 calculated in steps S21 to S24 are all constant, theheight of the imaging device may be different from that when thecalibration is performed. Here, when the height of the imaging devicechanges, since all the movement amounts 72 a of the image 70 a change,the height of the imaging device is calibrated by comparing with theactual movement amount of the mobile entity 100. When the height of theimaging device has been calibrated, all the movement amounts 72 a arethe same as the actual movement amount of the mobile entity 100, andtherefore the height of the imaging device is corrected untilerror_(N, p) in Formula (1) approaches 0. In the correction, forexample, a new height is set by trial and error, error_(N, p) iscalculated again, and the correction is repeated until error_(N, p)approaches 0. In addition, when the movement amount 72 a becomes largerthan the actual movement amount d_(N) of the mobile entity 100, it has ameaning that the actual height of the imaging device is lower than thatat the time of calibration, so that the height parameter of the imagingdevice is set low until error_(N, p) approaches 0.

Since the movement amount of the mobile entity estimated in step S25 isnot always accurate, and there is an error in tracking the moving pointin step S23, if error_(N, p) does not become 0, but approaches 0,calibration of the whole area of the image has been completed.

On the other hand, the bird's-eye view image 70 b shows a case where theroll angle of the imaging device is different from that at the time ofperforming the calibration. In this case, the movement amount 72 b ofthe moving point 71 obtained in steps S21 to S24 differs depending onthe area of the bird's-eye view image 70 b. For example, the movementamount 72 b of the moving point 71 on the left side of the bird's-eyeview image 70 b is larger than the movement amount 72 b on the rightside of the bird's-eye view image 70 b. In addition, the movement amount72 b of the moving point 71 at the center of the bird's-eye view image70 b is not different from the movement amount 72 a of the bird's-eyeview image 70 a. Therefore, when there is a pattern of the movementamount 72 b, the roll angle is corrected until error_(N, p) of themovement amount 72 b of the moving point 71 of the bird's-eye view image70 b becomes 0 because of an error in the roll angle.

The bird's-eye view image 70 c shows a case where the pitch angle of theimaging device is different from that at the time of performing thecalibration. In this case, the movement amount 72 c of the moving point71 obtained in steps S21 to S24 differs depending on the area of thebird's-eye view image 70 c. For example, the movement amount 72 c of themoving point 71 on the bird's-eye view image 70 c is larger than themovement amount 72 c below the bird's-eye view image 70 c. The closer tov=0, the larger the movement amount 72 c, and the farther from v=0, thesmaller the movement amount 72 c. Therefore, when there is a pattern ofthe movement amount 72 c, the pitch angle is corrected untilerror_(N, p) of the movement amount 72 c of the moving point 71 of thebird's-eye view image 70 c becomes zero because of an error in the pitchangle.

The bird's-eye view image 70 d shows a case where the yaw angle of theimaging device 12 is different from that at the time of performing thecalibration. In this case, the movement amount 72 d of the moving point71 obtained in steps S21 to S24 is constant, but moves in a directiondifferent from the v′ direction. Therefore, when there is a pattern ofthis movement amount 72 c, the yaw angle is corrected until the movementamount 72 d of the moving point 71 of the bird's-eye view image 70 dmoves in the same direction as the v′ direction due to an error in theyaw angle.

The bird's-eye view image 70 e shows a case where the distortion of theimaging devices 12 a, 12 b, . . . , 12 n has not been corrected. In thiscase, the direction of the movement amount 72 e of the moving point 71obtained in steps S21 to S24 is not constant. Therefore, the distortionis corrected until the direction of the movement amount 72 e of themoving point 71 of the bird's-eye view image 70 e becomes constant.

As described above, according to the present invention, the accuracy ofestimating the position of a mobile entity can be improved even whentravel is in progress or there is an error in the calibration performed.

It should be noted that the present invention is not limited to theembodiments described above, and includes various modifications. Forexample, the above-described embodiments are described in detail foreasy understanding of the present invention, and are not necessarilylimited to those including all the configurations described. Inaddition, a part of the configuration of one embodiment can be replacedwith the configuration of another embodiment, and the configuration ofanother embodiment can be added to the configuration of one embodiment.In addition, it is possible to add, delete, and replace anotherconfiguration with respect to a part of the configuration of each of theembodiments. In addition, each of the above-described configurations,functions, processing units, processing means, and the like may bepartially or entirely achieved by hardware by, for example, designingwith integrated circuits. In addition, each of the above-describedconfigurations, functions, and the like may be achieved by software byinterpreting and executing a program that achieves each function by theprocessor. Information such as a program, a table, and a file forachieving each function can be stored in a memory, a hard disk, arecording device such as an Solid State Drive (SSD), or a recordingmedium such as an IC card, an SD card, or a DVD.

REFERENCE SIGNS LIST

-   1 position estimation device-   12 imaging device-   13 information processing device-   14 image processing unit-   15 control unit-   16 memory-   17 display unit-   18 bus-   51 map information-   100 mobile entity-   212 road-   213 landmark

The invention claimed is:
 1. A mobile entity position estimation devicecomprising: a mobile entity; an imaging device provided in the mobileentity; and an information processing device configured to determine afirst movement amount by which a detection point being a same object hasmoved on based on a first image and a second image acquired by theimaging device and a second movement amount by which the mobile entityhas moved during the acquisition of the first image and the secondimage, determine accuracy of recognizing a detection point acquired bythe imaging device based on the first movement amount and the secondmovement amount, and estimate a position of the mobile entity based onthe accuracy of recognition and position information that pertains tothe detection point, wherein the first movement amount is a movementamount obtained by transforming a movement amount on a first image and asecond image being obtained into spatial coordinates of the mobileentity, wherein the recognition accuracy is a difference between thefirst movement amount and the second movement amount, wherein a weightof a detection point is determined from the difference, and wherein aposition of the mobile entity is estimated from the weight and positioninformation on the detection point.
 2. The mobile entity positionestimation device according to claim 1, wherein position information ofthe detection point is position information of the detection point onmap information.
 3. The mobile entity position estimation deviceaccording to claim 2, wherein calibration of an imaging device isperformed based on the difference.
 4. The mobile entity positionestimation device according to claim 3, wherein the imaging deviceincludes a plurality of imaging devices having different imagingdirections.
 5. A mobile entity position estimation method comprising:determining a first movement amount by which a detection point being asame object extracted from each of captured images of two or more framesacquired by an imaging device provided in a mobile entity has moved;determining a second movement amount by which the mobile entity hasmoved during the acquisition of the captured images of two or moreframes are acquired; determining accuracy of a detection point acquiredby the imaging device based on the first movement amount and the secondmovement amount; estimating a position of a mobile entity based on theaccuracy and position information on the detection point; determining aweight of the detection point based on the accuracy; and estimating aposition of a mobile entity from the weight and position information onthe detection point.
 6. The mobile entity position estimation methodaccording to claim 5, further comprising: calibrating an imaging devicebased on the accuracy, and estimating a position of a mobile entity.